#include "QEQueue.hpp"
#include "QAssert.hpp"
#include "QFPkg.hpp"
#include "QFPort.hpp"

namespace QP {

Q_DEFINE_THIS_MODULE("QEQueue")

QEQueue::QEQueue() noexcept : m_frontEvt(nullptr),
                              m_ring(nullptr),
                              m_end(0U),
                              m_head(0U),
                              m_tail(0U),
                              m_nFree(0U),
                              m_nMin(0U) {}

void QEQueue::init(std::shared_ptr<const QEvt> qSto[],
                   QEQueueCtr qLen) noexcept {
  m_frontEvt = nullptr;
  m_ring = qSto;
  m_end = qLen;
  m_head = (QEQueueCtr)0;
  m_tail = (QEQueueCtr)0;
  m_nFree = qLen;
  m_nMin = m_nFree;
}

void QEQueue::postFIFO(const std::shared_ptr<const QEvt>& e) noexcept {
  QF_CRIT_STAT_
  Q_REQUIRE_ID(200, e != nullptr);

  QF_CRIT_ENTRY_();

  if (m_frontEvt == nullptr) {                       // is the queue empty?
    m_frontEvt = e;                              // deliver event directly
  } else {            // queue is not empty, leave event in the ring-buffer
    // the queue must be able to accept the event (cannot overflow)
    Q_ASSERT(m_nFree != static_cast<QEQueueCtr>(0));

    QF_PTR_AT_(m_ring, m_head) = e; // insert event into the buffer (FIFO)
    if (m_head == static_cast<QEQueueCtr>(0)) {           // need to wrap?
      m_head = m_end;                                     // wrap around
    }
    --m_head;

    --m_nFree;                             // update number of free events
    if (m_nMin > m_nFree) {
      m_nMin = m_nFree;                         // update minimum so far
    }
  }
  QF_CRIT_EXIT_();
}

void QEQueue::postLIFO(const std::shared_ptr<const QEvt>& e) noexcept {
  QF_CRIT_STAT_

  QF_CRIT_ENTRY_();

  if (m_frontEvt != nullptr) {                   // is the queue not empty?
    // the queue must be able to accept the event (cannot overflow)
    Q_ASSERT(m_nFree != static_cast<QEQueueCtr>(0));

    ++m_tail;
    if (m_tail == m_end) {                       // need to wrap the tail?
      m_tail = static_cast<QEQueueCtr>(0);                // wrap around
    }

    QF_PTR_AT_(m_ring, m_tail) = m_frontEvt;   // buffer the old front evt

    --m_nFree;                             // update number of free events
    if (m_nMin > m_nFree) {
      m_nMin = m_nFree;                         // update minimum so far
    }
  }

  m_frontEvt = e;                        // stick the new event to the front
  QF_CRIT_EXIT_();
}

std::shared_ptr<const QEvt> QEQueue::get(void) {
  std::shared_ptr<const QEvt> e;
  QF_CRIT_STAT_

  QF_CRIT_ENTRY_();
  if (m_frontEvt == nullptr) {
    e = nullptr;
  } else {
    e = m_frontEvt;
    if (m_nFree != m_end) {           // any events in the the ring buffer?
      m_frontEvt = QF_PTR_AT_(m_ring, m_tail);   // remove from the tail
      if (m_tail == static_cast<QEQueueCtr>(0)) {  // need to wrap the tail?
        m_tail = m_end;               // wrap around
      }
      --m_tail;
      ++m_nFree;  // one more free event in the ring buffer
    } else {
      m_frontEvt = nullptr;  // the queue becomes empty
    }
  }
  QF_CRIT_EXIT_();
  return e;
}
}
